#include "industrial_robot_control/utils/realtime_utils.h"

namespace industrial_robot_control {
namespace utils {

// 实现头文件中声明的所有函数
void RealtimeUtils::setRealtimePriority(std::thread& thread, int priority) {
    // 函数实现（与头文件声明对应）
    if (priority < 1 || priority > 99) {
        throw std::invalid_argument("实时优先级必须在1-99范围内");
    }

    pthread_t pthread = thread.native_handle();
    struct sched_param param;
    param.sched_priority = priority;
    
    if (pthread_setschedparam(pthread, SCHED_FIFO, &param) != 0) {
        throw std::runtime_error("设置实时优先级失败: " + std::string(strerror(errno)));
    }
}

void RealtimeUtils::setCpuAffinity(std::thread& thread, const std::vector<int>& core_ids) {
    // 函数实现（与头文件声明对应）
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    
    for (int core : core_ids) {
        CPU_SET(core, &cpuset);
    }

    pthread_t pthread = thread.native_handle();
    if (pthread_setaffinity_np(pthread, sizeof(cpu_set_t), &cpuset) != 0) {
        throw std::runtime_error("设置CPU亲和性失败: " + std::string(strerror(errno)));
    }
}

bool RealtimeUtils::isRealtimeSupported() {
    // 函数实现（与头文件声明对应）
    struct sched_param param;
    param.sched_priority = 1;
    pthread_t pthread = pthread_self();
    
    int old_policy;
    struct sched_param old_param;
    
    if (pthread_getschedparam(pthread, &old_policy, &old_param) != 0) {
        return false;
    }
    
    int result = pthread_setschedparam(pthread, SCHED_FIFO, &param);
    if (result == 0) {
        pthread_setschedparam(pthread, old_policy, &old_param);
        return true;
    }
    
    return false;
}

} // namespace utils
} // namespace industrial_robot_control